#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/videoio.hpp>

ros::Publisher image_pub;

void publishImage(cv::Mat& frame)
{
    // 将OpenCV图像转换为ROS图像消息
    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
    cv_ptr->header.stamp = ros::Time::now();
    cv_ptr->header.frame_id = "camera_frame";  // 设置图像的frame_id
    cv_ptr->encoding = sensor_msgs::image_encodings::BGR8;  // 设置图像的编码
    cv_ptr->image = frame;

    // 发布图像消息
    image_pub.publish(cv_ptr->toImageMsg());
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "camera_publisher_node");
    ros::NodeHandle nh;

    // 创建OpenCV窗口，用于显示视频流
    cv::namedWindow("Camera Viewer", cv::WINDOW_NORMAL);

    // 创建一个ROS发布者，发布图像到话题"/camera/image_raw"
    image_pub = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 1);

    // 打开USB相机（假设相机设备ID为0）
    cv::VideoCapture cap(0);
    if (!cap.isOpened())
    {
        ROS_ERROR("无法打开USB相机");
        return -1;
    }

    // 设置图像大小（如果需要）
    int frame_width = 1280;  // 设置图像宽度
    int frame_height = 720;  // 设置图像高度
    cap.set(cv::CAP_PROP_FRAME_WIDTH, frame_width);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, frame_height);

    // 设置OpenCV窗口大小与图像分辨率一致
    cv::resizeWindow("Camera Viewer", frame_width, frame_height);
    
    // 循环读取图像并发布
    while (ros::ok())
    {
        cv::Mat frame;
        cap >> frame;  // 获取一帧图像
        if (frame.empty())
        {
            ROS_ERROR("捕获图像失败");
            break;
        }

        // 显示图像
        cv::imshow("Camera Viewer", frame);

        // 发布图像
        publishImage(frame);

        // 等待按键并处理OpenCV窗口事件
        cv::waitKey(1);

        // 调用ros::spinOnce来处理回调函数
        ros::spinOnce();
    }

    // 在节点退出时销毁OpenCV窗口
    cv::destroyWindow("Camera Viewer");

    return 0;
}
